# 这个例子展示了如何使用april标签代码中的四元检测代码在图像中找到矩形。 四元检测算法以非常稳健的方式检测矩形，并且比基于Hough变换的方法好得多。 例如，即使镜头失真导致这些矩形看起来弯曲，它仍然可以检测到矩形。 圆角矩形是没有问题的！
# (但是，这个代码也会检测小半径的圆，利用几何约束可将其筛除)
min_line1 = 25  # 矩形上下宽限制
min_line2 = 35  # 矩形左右长限制

import sensor, image, time, math
from pyb import LED
import lcd
from image import SEARCH_EX, SEARCH_DS

# 开启白灯
LED(1).on()
LED(2).on()
LED(3).on()

# 开关：画矩形边 画出识别结果 LCD显示透视效果 LCD显示缓冲画面 缓冲区显示透视效果
switch = [1,          1,         0,          0,          0]

sensor.reset()
sensor.set_vflip(1)
sensor.set_hmirror(1)
sensor.set_contrast(1)
sensor.set_gainceiling(16)
sensor.set_pixformat(sensor.GRAYSCALE) # 灰度更快(160x120 max on OpenMV-M7)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time = 2000)

# 模板加载 模板暂不提供，自行采集完后取消注释即可 下面的程序提供了采集模板的代码 先采集1到8的模板 再采集38和68 这两个模板是在8的基础上截取的
# templates = [image.Image(i) for i in ["/1.bmp","/2.bmp","/3.bmp","/4.bmp","/5.bmp","/6.bmp","/7.bmp","/8.bmp"]]
# template38 = image.Image("/38.bmp")
# template68 = image.Image("/68.bmp")

clock = time.clock()

# 计算两点直线距离
def dot_distance(dot1, dot2):
    return math.sqrt(math.pow(dot1[0]-dot2[0],2)+math.pow(dot1[1]-dot2[1],2))

# 计算两点中点坐标
def dot_middle(dot1, dot2):
    x = int(abs(dot1[0]+dot2[0])/2)
    y = int(abs(dot1[1]+dot2[1])/2)
    return x,y

lcd.init()

while(True):
    clock.tick()
    img = sensor.snapshot()
    img_copy = img.copy()
    img_copy.gamma_corr(0.3)    # gamma变换

    # 下面的`threshold`应设置为足够高的值，以滤除在图像中检测到的具有
    # 低边缘幅度的噪声矩形。最适用与背景形成鲜明对比的矩形。

    # 寻找矩形
    rect_list = img_copy.find_rects(threshold = 10000)
    if len(rect_list) > 4:
        continue
    for r in rect_list:
        dot_list = r.corners() # 矩形四点顺序：左下逆时针
        line1 = dot_distance(dot_list[0], dot_list[1])
        line2 = dot_distance(dot_list[1], dot_list[2])
        line3 = dot_distance(dot_list[2], dot_list[3])
        line4 = dot_distance(dot_list[3], dot_list[0])

        # 长宽约束
        if min_line1 > line1 or min_line2 > line2 or min_line1 > line3 or min_line2 > line4:
            continue
        if abs(line1-line3)>12 or abs(line2-line4)>12 or abs(line1-line2)>28 or abs(line3-line4)>28 or abs(line1-line4)>28 or abs(line3-line2)>28:continue
        if (line1+line3)>(line2+line4):continue

        if switch[0]:
            img.draw_line(dot_list[0][0], dot_list[0][1], dot_list[1][0], dot_list[1][1], 220)
            x,y = dot_middle(dot_list[0],dot_list[1])
            img.draw_string(x, y, str(int(line1)), 255, 0.95, -2)
            img.draw_line(dot_list[1][0], dot_list[1][1], dot_list[2][0], dot_list[2][1], 220)
            x,y = dot_middle(dot_list[1],dot_list[2])
            img.draw_string(x+2, y, str(int(line2)), 255, 0.95, -2)
            img.draw_line(dot_list[2][0], dot_list[2][1], dot_list[3][0], dot_list[3][1], 220)
            x,y = dot_middle(dot_list[2],dot_list[3])
            img.draw_string(x, y-12, str(int(line3)), 255, 0.95, -2)
            img.draw_line(dot_list[3][0], dot_list[3][1], dot_list[0][0], dot_list[0][1], 220)
            x,y = dot_middle(dot_list[3],dot_list[0])
            img.draw_string(x-14, y, str(int(line4)), 255, 0.95, -2)

        src = [(dot_list[3][0],dot_list[3][1]),(dot_list[2][0],dot_list[2][1]),(dot_list[1][0],dot_list[1][1]),(dot_list[0][0],dot_list[0][1])]
        if switch[4]:
            img1 = img
            if switch[3]:
                img = img1.copy()
        else:
            img1 = img.copy()

        img1.rotation_corr(corners=src).binary([(0,127)])   # 透视变换

        # 采集模板 nums:1~8 
        #img1.save("./1.bmp",roi=[14,8,129,105])

        # 由于3和6会与8混淆，所以要采集8的左roi和右roi，以此来将3与8和6与8区分开来 roi区可自行修改
        # 采集8的左roi
        #img1.save("./38.bmp",roi=[36,50,20,21])
        # 采集8的右roi
        #img1.save("./68.bmp",roi=[100,42,20,6])

        # 画出需要截取的roi区
        #img1.draw_rectangle(100, 42, 20, 6)

        # 模板匹配 采集完模板后取消注释即可
        result = 0
        # for i in range(8):
        #     r = img1.find_template(templates[i], 0.70, roi=[14,8,129,105],step=2, search=SEARCH_EX)
        #     if r:
        #         if i+1==3:
        #             # 区分3和8
        #             r = img1.find_template(template38, 0.4, roi=[36, 50, 20, 21], step=2, search=SEARCH_EX)
        #             if r:
        #                 result = 8
        #                 break
        #         if i+1==6:
        #             # 区分6和8
        #             r = img1.find_template(template68, 0.1, roi=[100,42,20,6], step=2, search=SEARCH_EX)
        #             if r:
        #                 result = 8
        #                 break
        #         result=i+1
        #         break

        if result != 0 and switch[1]:
            x,y = dot_middle(dot_list[0],dot_list[1])
            x1,y1 = dot_middle(dot_list[2],dot_list[3])
            x,y = dot_middle((x,y),(x1,y1))
            # 画出中点
            #img.draw_circle(x,y,2,0)
            # 画出坐标
            #img.draw_string(x, y+1, str(x)+'\n'+str(y), 0, 0.95, -2)
            # 画出矩形各边长
            img.draw_string(dot_list[0][0], dot_list[0][1], str(result), 255, 0.95, -2)
        if result != 0 and switch[2]:
            lcd.display(img1)

    #img.draw_string(0,0,str(clock.fps()))
    if switch[3]:
        lcd.display(img)

